#include <iostream>
#include <thread>
#include "opencv2/opencv.hpp"
#include "opencv2/tracking.hpp"
#include "onvifClient.h"
#include "dome_pid.h"

int main()
{
    cv::VideoCapture capture;
    capture.open("rtsp://192.168.3.210/qiu");

    if (!capture.isOpened())
        std::cerr << "Unable to open the RTSP stream" << std::endl;

    cv::Mat frame;

    std::thread thread([&](){
        while (true) 
        {
            if (!capture.read(frame)) 
            {
                std::cerr << "Unable to read next frame." << std::endl;
                break;
            }

            // cv::imshow("RTSP Stream", frame);
            // cv::waitKey(1);
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }
    });
    thread.detach();


    DomePid domePid;

    cvcam::onvif::OnvifClient client;
    client.onvifInit("http://192.168.3.240/onvif/device_service", "admin", "Aa123456789", true);

    cvcam::onvif::OnvifClient::Vector3f status;
    client.onvifPtzGetStatus(status, "Profile_1");


    cv::Rect2d rect;
    //cv::Ptr<cv::Tracker> tracker = cv::TrackerMOSSE::create();
    cv::Ptr<cv::Tracker> tracker;
    while (true)
    {
        // if (!frame.empty() && rect.area() == 0)
        // {
        //     rect = cv::selectROI("selectROI Stream", frame);
        //     std::cout << "selectROI ROI: " << rect << std::endl;

        //     // tracker = cv::TrackerMOSSE::create();
        //     // tracker = cv::TrackerKCF::create();
        //     // tracker->init(frame, rect);
        // }

        // if (!frame.empty() && rect.area() > 0)
        if (!frame.empty())
        {
            // bool ok = tracker->update(frame, rect);
            bool ok = true;
            if (ok) 
            {
                // cv::rectangle(frame, rect, cv::Scalar(255, 0, 0), 2, 1);

                {
                    cv::Rect2d normalizedRect(rect.x / frame.cols, rect.y / frame.rows, rect.width / frame.cols, rect.height / frame.rows);
                    // domePid.setDomeInX(normalizedRect.x + normalizedRect.width / 2);
                    // domePid.setDomeInY(normalizedRect.y + normalizedRect.height / 2);

                    // domePid.setDomeInZ(normalizedRect.width * normalizedRect.height);

                     cvcam::onvif::OnvifClient::Vector3f status;
                    client.onvifPtzGetStatus(status, "Profile_1");

                    std::cout << "status.x: " << status.x << " status.y: " << status.y << " status.z: " << status.z << std::endl;

                    domePid.setDomeInX(status.x);
                    domePid.setDomeInY(status.y);
                    domePid.setDomeInZ(status.z);
                    domePid.compute();

                    float speedX = domePid.getDomeOutSpeedX();
                    float speedY = domePid.getDomeOutSpeedY();
                    float speedZ = domePid.getDomeOutSpeedZ();

                    client.onvifPtzContinuousMove( cvcam::onvif::OnvifClient::Vector3f(speedX, speedY, speedZ), "Profile_1");
                    std::cout << "speedX: " << speedX << " speedY: " << speedY << " speedZ: " << speedZ << std::endl;
                }
            }
            else
                client.onvifPtzStop("Profile_1");

            cv::imshow("tracker Stream", frame);
            if (cv::waitKey(1) >= 0) 
            {
                rect = cv::Rect2d();
                cv::destroyWindow("tracker Stream");

                client.onvifPtzStop("Profile_1");
            } 
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    return 0;
}
